close all; clear; clc;

a1 = 1; a2 = 1;

Alpha = [pi/2, 0];
A = [0.0, a1];
D = [0.0, 0.0];
Theta = [0, 0];

DH_para.Alpha = Alpha;
DH_para.A = A;
DH_para.D = D;
DH_para.Theta = Theta;

L(1) = Link('alpha',Alpha(1),'a', A(1),'d',D(1));L(1).offset = Theta(1); L(1).qlim=[-2 * pi, 2 * pi];L(1).mdh = 1;
L(2) = Link('alpha',Alpha(2),'a', A(2),'d', D(2));L(2).offset = Theta(2);L(2).qlim=[-2 * pi, 2 * pi];L(2).mdh = 1;

robot = SerialLink(L, 'name', 'robot');
robot.tool = transl(a2, 0, 0) * trotz(pi/2) * trotx(pi/2);

p_start = [1.2; 0; 0.6];
p_end = [0.4; 0; -0.1];

q_start = ikine(p_start);
q_end = ikine(p_end);

qi = q_start;
p_i = FKine(qi, DH_para);

eps = 1e-3;

p_ob = [0.75; 0.0; 0.3; 0.1];

k_rad = 3;
k_rep = 0.01;
k_attr = 0.01;

i = 0;

qs = [qi'];
ps = [p_i'];
while norm(p_i - p_end) > eps
    [line1, line2] = cal_lines(qi);
    p_i = FKine(qi, DH_para);
    [d1, p1, t1] = point_to_lineSegment(p_ob, line1);
    [d2, p2, t2] = point_to_lineSegment(p_ob, line2);
    d3 = point_to_point(p_ob, p_i);
    
    F1 = repulsive_force(p_ob, p1, k_rad, k_rep);
    F2 = repulsive_force(p_ob, p2, k_rad, k_rep);
    F3 = attractive_force(p_i, p_end, k_attr);
    
    [J, J1, J2] = cal_Jacobian(qi, t1, t2);
    
    delta1 = pinv(J1) * F1;
    delta2 = pinv(J2) * F2;
    delta3 = pinv(J) * F3;
    qi = qi + delta1 + delta2 + delta3;
    p_i = FKine(qi, DH_para);
    
    ps = [ps; p_i'];
    qs = [qs; qi'];
    
    i = i + 1;
    if i > 1000
        disp("planning failed");
        break;
    end
end

figure(1);
plot3(ps(:, 1), ps(:, 2), ps(:, 3));
plot_sphere(p_start, 0.05, 'g');
plot_sphere(p_end, 0.05, 'r');
plot_sphere(p_ob(1:3), p_ob(4));
robot.plot(qs);